#pragma once

#include <cmath>
#include <array>
#include <cassert>
#include <cfloat>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif


namespace ts {
    template<typename T, size_t N>
    class Buf {
    public:
        using self = Buf;
        const static size_t Size = N;

        T &operator[](size_t i) { return this->m_data[i]; }

        const T &operator[](size_t i) const { return this->m_data[i]; }

        T &data(size_t n) { return this->m_data[n]; }

        const T &data(size_t n) const { return this->m_data[n]; }

        T *data() { return this->m_data; }

        const T *data() const { return this->m_data; }

    private:
        T m_data[N];
    };

    template<typename T, size_t M, size_t N>
    class Mat : public Buf<T, M * N> {
    public:
        using self = Mat;
        using supper = Buf<T, M * N>;
        const static size_t Rows = M;
        const static size_t Cols = N;

        using supper::data;

        T &data(size_t m, size_t n) { return this->data(m * N + n); }

        const T &data(size_t m, size_t n) const { return this->data(m * N + n); }
    };

    /**
     * \brief means column vector
     */
    template<typename T, size_t N>
    class Vec : public Buf<T, N> {
    public:
        using self = Vec;
        using supper = Buf<T, N>;
    };

    template<typename T>
    class Trans2D : public Mat<T, 3, 3> {
    public:
        using self = Trans2D;
        using supper = Mat<T, 3, 3>;

        Trans2D() {}

        Trans2D(
                T r1c1, T r1c2, T r1c3,
                T r2c1, T r2c2, T r2c3,
                T r3c1, T r3c2, T r3c3) {
            this->data(0) = r1c1;
            this->data(1) = r1c2;
            this->data(2) = r1c3;
            this->data(3) = r2c1;
            this->data(4) = r2c2;
            this->data(5) = r2c3;
            this->data(6) = r3c1;
            this->data(7) = r3c2;
            this->data(8) = r3c3;
        }
    };

    template<typename T>
    class Vec3D : public Vec<T, 3> {
    public:
        using self = Vec3D;
        using supper = Vec<T, 3>;

        const T &x() const { return this->data(0); };

        const T &y() const { return this->data(1); };

        const T &z() const { return this->data(2); };

        T &x() { return this->data(0); };

        T &y() { return this->data(1); };

        T &z() { return this->data(2); };

        Vec3D() : Vec3D(0, 0, 0) {}

        Vec3D(T x, T y, T z) {
            this->data(0) = x;
            this->data(1) = y;
            this->data(2) = z;
        }

        Vec3D(T x, T y) : self(x, y, T(1)) {}

    };

    template<typename T>
    class Vec2D : public Vec<T, 2> {
    public:
        using self = Vec2D;
        using supper = Vec<T, 2>;

        T x() const { return this->data(0); };

        T y() const { return this->data(1); };

        T &x() { return this->data(0); };

        T &y() { return this->data(1); };

        Vec2D() : Vec2D(0, 0) {}

        Vec2D(T x, T y) {
            this->data(0) = x;
            this->data(1) = y;
        }
    };


    /**
     * \brief get A dot B
     * \tparam T dtype
     * \param A left matrix
     * \param B right matrix
     * \return A dot B
     */
    template<typename T>
    Trans2D<T> dot(const Trans2D<T> &A, const Trans2D<T> &B) {
#define __INNER_PRODUCT_Am_Bn(m, n) (A.data(m, 0) * B.data(0, n) + A.data(m, 1) * B.data(1, n) + A.data(m, 2) * B.data(2, n))
        return Trans2D<T>(
                __INNER_PRODUCT_Am_Bn(0, 0),
                __INNER_PRODUCT_Am_Bn(0, 1),
                __INNER_PRODUCT_Am_Bn(0, 2),
                __INNER_PRODUCT_Am_Bn(1, 0),
                __INNER_PRODUCT_Am_Bn(1, 1),
                __INNER_PRODUCT_Am_Bn(1, 2),
                __INNER_PRODUCT_Am_Bn(2, 0),
                __INNER_PRODUCT_Am_Bn(2, 1),
                __INNER_PRODUCT_Am_Bn(2, 2)
        );
#undef __INNER_PRODUCT_Am_Bn
    }

    /**
     * \brief A = B dot A
     * \tparam T dtype
     * \param A stacked matrix
     * \param B add new trasnformation
     */
    template<typename T>
    void stack(Trans2D<T> &A, const Trans2D<T> &B) {
        A = dot(B, A);
    }

    /**
     * \brief
     * \tparam T dtype
     * \param M transmation matrix
     * \param p 2D point in 3D-vector
     * \return M dot p
     */
    template<typename T>
    Vec3D<T> transform(const Trans2D<T> &M, const Vec3D<T> &p) {
#define __INNER_PRODUCT_Mn_p(n) (M.data(n, 0) * p.data(0) + M.data(n, 1) * p.data(1) + M.data(n, 2) * p.data(2))
        return Vec3D<T>(
                __INNER_PRODUCT_Mn_p(0),
                __INNER_PRODUCT_Mn_p(1),
                __INNER_PRODUCT_Mn_p(2)
        );
#undef __INNER_PRODUCT_Mn_p
    }

    /**
     * \brief
     * \tparam T dtype
     * \param M transmation matrix
     * \param p 2D point in 3D-vector
     * \return M dot p
     */
    template<typename T>
    Vec2D<T> transform(const Trans2D<T> &M, const Vec2D<T> &p) {
        auto transed = transform < T > (M, Vec3D<T>(p.data(0), p.data(1), T(1)));
        return Vec2D<T>(transed.data(0), transed.data(1));
    }

    namespace affine {
        /**
         * \brief get inverse transformation matrix, perform p(x', y') = M dot p(x, y)
         * \tparam T dtype
         * \param M origin matrix
         * \return the inverse matrix
         */
        template<typename T>
        Trans2D<T> inverse(const Trans2D<T> &M) {
            assert(M[6] == 0);
            assert(M[7] == 0);
            assert(M[8] == 1);
            T t3t1_t0t4 = M[3] * M[1] - M[0] * M[4];
            if (t3t1_t0t4 < FLT_EPSILON && t3t1_t0t4 > -FLT_EPSILON) t3t1_t0t4 = FLT_EPSILON * 2;
            T recip_t3t1_t0t4 = 1 / t3t1_t0t4;
            return Trans2D<T>(
                    -M[4] * recip_t3t1_t0t4, M[1] * recip_t3t1_t0t4, -(M[1] * M[5] - M[4] * M[2]) * recip_t3t1_t0t4,
                    M[3] * recip_t3t1_t0t4, -M[0] * recip_t3t1_t0t4, -(M[3] * M[2] - M[0] * M[5]) * recip_t3t1_t0t4,
                    T(0), T(0), T(1)
            );
        }

        /**
         * \brief get transformation matrix, perform p(x', y') = M dot p(x, y)
         * \tparam T dtype
         * \param degree degree
         * \return the rotatation matrix
         */
        template<typename T>
        Trans2D<T> rotate(T degree) {
            const static double pi_dev_180 = M_PI / 180.0;
            auto cos_degree = std::cos(degree * pi_dev_180);
            auto sin_degree = std::sin(degree * pi_dev_180);
            return Trans2D<T>(
                    T(cos_degree), T(-sin_degree), T(0),
                    T(sin_degree), T(cos_degree), T(0),
                    T(0), T(0), T(1)
            );
        }

        /**
         * \brief get transformation matrix, perform p(x', y') = M dot p(x, y)
         * \tparam T dtype
         * \param tx shift on x-dim
         * \param ty shift on y-dim
         * \return the traslation matrix
         */
        template<typename T>
        Trans2D<T> translate(T tx, T ty) {
            return Trans2D<T>(
                    T(1), T(0), tx,
                    T(0), T(1), ty,
                    T(0), T(0), T(1)
            );
        }

        /**
         * \brief get identity matrix, perform p(x, y) = M dot p(x, y)
         * \tparam T dtype
         * \return indentiy matrix
         */
        template<typename T>
        Trans2D<T> identity() {
            return Trans2D<T>(
                    T(1), T(0), T(0),
                    T(0), T(1), T(0),
                    T(0), T(0), T(1)
            );
        }

        /**
         * \brief get transformation matrix, perform p(x', y') = M dot p(x, y)
         * \tparam T dtype
         * \param sx scale on x-dim
         * \param sy scale on y-dim
         * \return the scale matrix
         */
        template<typename T>
        Trans2D<T> scale(T sx, T sy) {
            return Trans2D<T>(
                    sx, T(0), T(0),
                    T(0), sy, T(0),
                    T(0), T(0), T(1)
            );
        }

        /**
         * \brief get transformation matrix, perform p(x', y') = M dot p(x, y)
         * \tparam T dtype
         * \param s scale on x-dim and y-dim
         * \return the scale matrix
         */
        template<typename T>
        Trans2D<T> scale(T s) {
            return scale < T > (s, s);
        }

        /**
         * \brief get transformation matrix, perform p(x', y') = M dot p(x, y)
         * \tparam T dtype
         * \param shx shear on x-dim
         * \param shy shear on y-dim
         * \return the shear matrix
         */
        template<typename T>
        Trans2D<T> shear(T shx, T shy) {
            return Trans2D<T>(
                    T(1), shx, T(0),
                    shy, T(1), T(0),
                    T(0), T(0), T(1)
            );
        }

        /**
         * \brief get transformation matrix, perform p(x', y') = M dot p(x, y)
         * \tparam T dtype
         * \param sh shear on x-dim
         * \return the shear matrix
         */
        template<typename T>
        Trans2D<T> shear_x(T sh) {
            return shear<T>(sh, 0);
        }

        /**
         * \brief get transformation matrix, perform p(x', y') = M dot p(x, y)
         * \tparam T dtype
         * \param sh shear on y-dim
         * \return the shear matrix
         */
        template<typename T>
        Trans2D<T> shear_y(T sh) {
            return shear<T>(0, sh);
        }

        /**
         * \brief get transformation matrix, perform p(x', y') = M dot p(x, y)
         * \tparam T dtype
         * \param fx if reflect on x-dim
         * \param fy if reflect on y-dim
         * \return the reflection matrix
         */
        template<typename T>
        Trans2D<T> reflect(bool fx, bool fy) {
            return Trans2D<T>(
                    fx ? T(-1) : T(1), T(0), T(0),
                    T(0), fy ? T(-1) : T(1), T(0),
                    T(0), T(0), T(1)
            );
        }

        /**
         * \brief get transformation matrix, perform p(x', y') = M dot p(x, y)
         * \tparam T dtype
         * \return the reflection matrix aouble origin
         */
        template<typename T>
        Trans2D<T> reflect_about_origin() {
            return Trans2D<T>(
                    T(-1), T(0), T(0),
                    T(0), T(-1), T(0),
                    T(0), T(0), T(1)
            );
        }

        /**
        * \brief get transformation matrix, perform p(x', y') = M dot p(x, y)
        * \tparam T dtype
        * \return the reflection matrix about x-axis
        */
        template<typename T>
        Trans2D<T> reflect_about_x_axis(bool fx, bool fy) {
            return Trans2D<T>(
                    T(1), T(0), T(0),
                    T(0), T(-1), T(0),
                    T(0), T(0), T(1)
            );
        }

        /**
        * \brief get transformation matrix, perform p(x', y') = M dot p(x, y)
        * \tparam T dtype
        * \return the reflection matrix about y-axis
        */
        template<typename T>
        Trans2D<T> reflect_about_y_axis(bool fx, bool fy) {
            return Trans2D<T>(
                    T(-1), T(0), T(0),
                    T(0), T(1), T(0),
                    T(0), T(0), T(1)
            );
        }
    }

}

